-------------------
-- Text Display
-------------------
function write (string, line)
  line = line or 1
  line = nxt.min(nxt.max(line, 1), 8)
  nxt.DisplayText(padded(string), 0, (line - 1) * 8)
end

function padded (string) 
  return deBoolean(string) .. "               "
end

cls = nxt.DisplayClear

abs = nxt.abs

function deBoolean (var)
  if isBoolean(var) then
    if var then
      return "true"
    else 
      return "false"
    end
  else
    return var
  end
end

function isBoolean (var) 
  return var == false or var == true
end

-------------------
-- Motors
-------------------

-- Motor Designations
A = 1
B = 2
C = 3

__defaultSpeed = 100

-- Motor Commands
function stop (motor)
  forward(motor, 0)
end

function float (motor)
  nxt.OutputSetSpeed(motor, 0, 0)
end

function forward (motor, speed) 
  speed = speed or __defaultSpeed
  nxt.OutputSetSpeed(motor, 32, speed)
  nxt.OutputSetRegulation(motor, 1, 1)
end

function backward (motor, speed)
  speed = speed or __defaultSpeed
  forward (motor, -speed)
end

function speedReading (motor)
  return nxt.OutputGetStatus(motor)
end

function rotationCounts (motor)
  _,counts = nxt.OutputGetStatus(motor)
  return counts
end

function resetRotationCount (motor)
  stop(motor)
  nxt.OutputResetTacho(motor, 1)
end

function resetAllMotors()
  resetRotationCount(A)
  resetRotationCount(B)
  resetRotationCount(C)
end

--------------
-- Buttons
--------------

-- Button Designations
ENTER = 8
ESCAPE = 1
LEFT = 4
RIGHT = 2

-- Button Command
isPressed = function (button) return nxt.ButtonRead() == button end

-------------------
-- Basic Sensors
-------------------

function reading (port)
  local val
  _,_,_,val = nxt.InputGetStatus(port)
  return val
end

function isTouched (port) 
  nxt.InputSetType(port, 1, 0x20)
  return reading(port) == 1
end

function activeLightReading (port) 
  nxt.InputSetType(port, 5, 0x80)
  return reading(port)
end

function passiveLightReading (port) 
  nxt.InputSetType(port, 6, 0x80)
  return reading(port)
end

-------------------
-- Sonar
-------------------

nxt.I2Cversion    = string.char( 0x02, 0x00 )
nxt.I2Cproduct    = string.char( 0x02, 0x08 )
nxt.I2Ctype       = string.char( 0x02, 0x10 )
nxt.I2Ccontinuous = string.char( 0x02, 0x41, 0x02 )
nxt.I2Cdata       = string.char( 0x02, 0x42 )

__sonarReady = {false,false,false,false}

-- Regarding the delay:
--
-- I found experimentally that forcing a wait of 25 ms between sonar readings
-- gives really nice performance.  A delay of, say, 12 ms produces occasional
-- lost values.  A delay of 1 ms results in virtually all values lost.
--
-- I made it an optional parameter because it is imaginable that the delay
-- might be incorporated into other parts of the program.  This gives a nice
-- default with good behavior while still allowing flexibility on the part
-- of the programmer.

function sonarReading (port, delay)
  delay = delay or 25
  if not __sonarReady[port] then
    setupI2C(port)
    nxt.I2CSendData(port, nxt.I2Cproduct, 8)

    nxt.I2CSendData(port, nxt.I2Ccontinuous, 0)
    waitI2C(port)
    __sonarReady[port] = true
  end

  sleep(delay)
  nxt.I2CSendData(port, nxt.I2Cdata, 8)
  waitI2C(port)
  return string.byte(nxt.I2CRecvData(port, 8))
end

function waitI2C (port)
  repeat until nxt.I2CGetStatus(port) == 0
end

function setupI2C (port)
  nxt.InputSetType(port,2)
  nxt.InputSetDir(port,1,1)
  nxt.InputSetState(port,1,1)
  nxt.I2CInitPins(port)
end

-------------------
-- Time
-------------------

time = nxt.TimerRead

-- Rethink relative to integer overflow
function sleep(milliseconds)
  local stop = time() + milliseconds
  repeat until time() >= stop
end

-------------------
-- Letting you know the prelude arrived
-------------------

nxt.DisplayClear()
nxt.DisplayText("Prelude loaded...")

function __main()
cls()

forward (A)
forward (B)
repeat 
 if isTouched(1) then
 forward (A)
 forward (B, 50)
 elseif isTouched(2) then
 backward (A)
 backward (B, 20)
 else 
   forward(A)
   forward(B)
 end
 until rotationCounts(A) > 9000
 forward (A,80)
 forward (B,100)
repeat
 if isTouched(1) then
 forward (A)
 forward (B, 50)
 elseif isTouched(2) then
 backward (A)
 backward (B, 20)
 else 
   forward(A)
   forward(B)
 end
 until rotationCounts(A) > 20000
 forward (A)
 forward (B)
repeat
 if isTouched(1) then
 forward (A)
 forward (B, 50)
 elseif isTouched(2) then
 backward (A)
 backward (B, 20)
 else 
   forward(A)
   forward(B)
 end
 until rotationCounts(A) > 29000
stop (A)
stop (B)

end
repeat
cls()
resetAllMotors()
write("Press the ORANGE", 2)
write("button to START", 3)
write("your program", 4)
write("Press and hold", 6)
write("the GRAY button", 7)
write("when FINISHED", 8)
repeat until isPressed(ENTER)
__main()
until false
